#include <cmath>
#include "conio.h"
#include "cv.h"
#include "highgui.h"
#include "stdio.h"
#include "stdlib.h"
#include "windows.h"
#include <tchar.h>
//#include <time.h>
//#include "Serial.h"
//aaa
using namespace std;

IplImage *frame, *colimgbot, *monoimgbot, *ref;
uchar *datahsv,*datamono,*refData;

int timeOut = 1, heighthsv, widthhsv, stephsv, channelshsv, stepmono, channelsmono, currentR, currentG, currentB;
int x_top, y_top, x_bottom, y_bottom, pix_no,tol_x = 10, x_wanted, indexH, indexS, indexV, indexMono;

double toleranceH = 14, toleranceS = 25, toleranceV = 75, alfa, lng;

bool showOriginal = false;

//CSerial serial;
//LONG    lLastError = ERROR_SUCCESS;


void setRefValsRGB(int R, int G, int B)
{
	currentR = R;
	currentG = G;
	currentB = B;

	printf("currentR: %d", currentR);
	printf("currentG: %d", currentG);
	printf("currentB: %d", currentB);

	ref = cvCreateImage(cvSize(100, 100), IPL_DEPTH_8U, 3);
			
	refData = (uchar *)ref->imageData;
	
	for (int p = 0 ; p < (ref->width) ; p++)
	{
		for(int q = 0 ; q < (ref->height) ; q++)
		{
			refData[p*(ref->widthStep)+q*(ref->nChannels)] = B;
			refData[p*(ref->widthStep)+q*(ref->nChannels)+1] = G;
			refData[p*(ref->widthStep)+q*(ref->nChannels)+2] = R;
		}
	}
	cvCvtColor(ref,ref,CV_RGB2HSV);	
}
//
void on_mouse( int event, int x, int y, int flags, void* param )
{
	switch( event )
    {
		case CV_EVENT_LBUTTONDOWN:
        {
			uchar *tmpData = (uchar *)frame->imageData;
			setRefValsRGB(tmpData[((y-1)*(frame->width*3)) + x*3 + 2],tmpData[((y-1)*(frame->width*3)) + x*3 + 1],
				tmpData[((y-1)*(frame->width*3)) + x*3]);
		}
	 }
}

void kretanje(char motor, int brzina)
{
	if(motor == 'l' || motor=='L')
	{
		if(brzina<0)
		{
			brzina=brzina*(-1);
			brzina = brzina + 128;
		}
		else
			brzina = brzina + 192;
	}
	else
	{
		if(motor == 'd' || motor == 'D')
		{
			if(brzina<0)
			{
				brzina=brzina*(-1);
				brzina = brzina + 64;
			}
		}
	}
	
	char* aa = (char*)&brzina;	
    //lLastError = serial.Write(aa);
	//printf("\nMotor: %c\n",motor);
	//printf("\nBrzina: %d\n\n",brzina);
}

int main()
{
	if(showOriginal)
	{
		cvNamedWindow("original", CV_WINDOW_AUTOSIZE);
		cvSetMouseCallback("original",on_mouse);
	}
	cvNamedWindow("SEARCH",CV_WINDOW_AUTOSIZE);

	CvCapture* capture = cvCaptureFromCAM( CV_CAP_ANY );
	frame = cvQueryFrame( capture );

	colimgbot = cvCreateImage( cvGetSize(frame), 8, 3 );
	//monoimgbot = cvCreateImage( cvGetSize(frame), 8, 1 );
	monoimgbot = cvCreateImage( cvSize(frame->width/10,frame->height/10), 8, 1 );

	//setRefValsRGB(169,50,59);	// Bokijeva kamera u shkoli
	setRefValsRGB(116,30,18);	// Logitech kuci

	//lLastError = serial.Open(_T("COM1"),0,0,false);
    //lLastError = serial.Setup(CSerial::EBaud2400,CSerial::EData8,CSerial::EParNone,CSerial::EStop1);
	//lLastError = serial.SetupHandshaking(CSerial::EHandshakeOff);

	while( 1 ) 
	{		
		frame = cvQueryFrame( capture );

		heighthsv = colimgbot->height;
		widthhsv = colimgbot->width;
		stephsv = colimgbot->widthStep;
		channelshsv = colimgbot->nChannels;
		datahsv = (uchar *)colimgbot->imageData;

		stepmono = monoimgbot->widthStep;
		channelsmono = monoimgbot->nChannels;
		datamono = (uchar *)monoimgbot->imageData;

		//--------------------------

		//cvSmooth(frame,frame, CV_GAUSSIAN,15,0,0,0);
		cvCvtColor(frame,colimgbot,CV_RGB2HSV);

		x_wanted = widthhsv/2;

		//clock_t start;
		//start = clock();

		for(int i = 0, p = 0 ; i < heighthsv ; i+=10, p++)
		{
			for(int j = 0, q = 0 ; j < widthhsv ; j+=10, q++)
			{
				indexH = i*stephsv+j*channelshsv;
				indexS = indexH + 1;
				indexV = indexH + 2;

				//indexMono = i*stepmono+j*channelsmono;
				indexMono = p*stepmono+q*channelsmono;

				if	(	(	(	datahsv[indexH]	<	refData[0]	+	toleranceH	) && (	datahsv[indexH]	>	refData[0]	)	) &&
						(	(	datahsv[indexS]	<	refData[1]	+	toleranceS	) && (	datahsv[indexS]	>	refData[1]	-	toleranceS	)	) &&
						(	(	datahsv[indexV]	<	refData[2]	+	toleranceV	) && (	datahsv[indexV]	>	refData[2]	-	toleranceV	)	)	)
				{
					datamono[indexMono] = 255;
					x_top = j;
					y_top = i;

					if(x_top > (x_wanted - tol_x) && x_top < (x_wanted + tol_x))
					{
						kretanje('d',12);
						kretanje('l',12);
					}
					
					if(x_top < (x_wanted - tol_x))
					{
						kretanje('l',1);
						kretanje('d',12);
					}

					if(x_top > (x_wanted + tol_x))
					{
						kretanje('l',12);
						kretanje('d',1);												
					}

					pix_no = heighthsv - y_top;

					alfa = pix_no * 0.0625;
					lng = tan((alfa + 41.52)*3.14159/180) * 30.5;
				}
				else
					datamono[indexMono] = 0;
			}
		}

		//int time = clock() - start;

		//cvErode(monoimgbot,monoimgbot,0,2);
		//cvDilate( monoimgbot,monoimgbot,0,5);

		if( (cvWaitKey(timeOut)) == 27 )	
			break;
		
		if(showOriginal)
			cvShowImage("original",frame);
		cvShowImage("SEARCH",monoimgbot);		
	}

	if(showOriginal)
		cvDestroyWindow("original");
	cvDestroyWindow("SEARCH");

	//serial.Close();
	
	return 0;
}